1. Field of Invention
The present invention relates generally to the field of Simultaneous Localization And Mapping (SLAM) and the methodologies used therein, in particular SLAM used in robotic or human carried cameras.
2. Description of Related Art
The present invention relates to Simultaneous Localization And Mapping (SLAM). SLAM and the methodologies used therein can be used in a wide variety of applications, in particular robotic vision or human carried cameras, often used in military applications.
Simultaneous localization and mapping (SLAM) and its related methodologies can be used by robotic machine vision to construct a map of position and orientation within an unknown topographical environment, all the while keeping track of the current position. This problem is compounded by two main sources of error: (1) noise error due to electrical measurements of mechanical, inertial, optical, acoustic or magnetic sensors and any associated 10 (input/output) devices; and, (2) measurement error of the state of the environment or object (e.g. system) being measured, due to a variety of factors including the fact the state of a system may be non-linear, in that what occurred at one instant of time does not necessarily predict what will happen in another instant of time.
To give a concrete, albeit simplistic, example of error, a person who remembers their initial position at one point in time and is then blindfolded is likely to accurately construct their position and orientation at another point in time after only one complete spin about the initial position, or after being marched in a short straight line from the initial position, but is unlikely to construct their position and orientation if given numerous spins and taken in numerous random directions after being blindfolded. If the person is introduced to noise during this process, e.g. if the person's initial vision is made blurry, then any error is compounded.
Further, in SLAM-based environments if the measured distance and direction traveled by an object in an environment has a slight inaccuracy, then at the next iteration of map building the environment any features being added to the map can contain corresponding positional errors, that, if unchecked, can build cumulatively, which grossly distorts the map. Various techniques to compensate for this and other distortions exist, such as recognizing features that the object in the environment has come across previously, and re-skewing recent parts of the map to make sure the two instances of that feature become one. Often these techniques employ statistical methods to give some direction to seemingly uncorrelated events, hence are termed stochastic methods, and include the use of so-called Kalman filters.
Broadly speaking, a Kalman filter methodology (or Kalman filter) can be thought of an estimation method that resembles a predictor-corrector method for solving a numerical problem, the Kalman filter having a time update predictor cycle for projecting and predicting a state estimate of a system ahead in time, and a measurement update cycle for correcting the projected estimate made by the time update cycle, based on actual measurements made at a future point in time.
The Extended Kalman Filter (EKF) methodology is employed when the process to be estimated and/or the measurement relationship to the process is non-linear. Typically, an estimate about a current estimation is linearized, akin to a Taylor series, using partial differential equations, yielding a two-cycle time update prediction cycle and measurement update correction cycle analogous to the Kalman filter.
Prior solutions in using SLAM to navigate accurately have depended on certain SLAM related software and hardware, for example, using hardware that senses at wide angles, and using numerous multiple observations, such as tracking every feature that can be tracked in the hopes that a sufficient number of features will be good navigation features. In an EKF, usually the most common and accurate methodology used in SLAM, the solution convergence in SLAM for an EKF approach is judged at an associated computational cost of N2 (O(N^2)), or quadratic in Landau notation), where N is the number of features currently being tracked. This computation cost is rather excessive for nearly all but very powerful computers; some attempts to handle this computation load have resorted to deleting features from memory that have not been used for some specified period of time. Even so, navigational units using SLAM methodology along these lines are currently restricted to relatively short times and slower motion.
Another problem in the prior art of SLAM methodologies is that for human carried cameras, smooth motion is not always feasible. SLAM methodologies such as Unscented Kalman Filter (UKF) and EKF methodologies do not adapt well to non-smooth motion; in the former case, UKFs potentially create physically erroneous diverging solutions, while in the latter case, EKFs do not work well with non-smooth inputs from a sensor being used to sense an environment.
Finally, current SLAM routines used in navigation do not have the capability to handle one time inputs such as GPS satellite measurement data, or to take a currently tracked feature and recognize its location on a map. Thus SLAM navigation is primarily at present used for relative navigation; even SLAM that starts off with a known location will navigate only in a relative manner. This restriction to relative navigation places no bounds on the error growth in determining sensor(s) location.
In sum, what is not found in the prior art is a system and method to improve upon present SLAM methodologies, that the present invention addresses, improving upon some of the defects explained above and/or in addition as the claimed invention is explained below in the remaining specification.